perm filename HANDSE.SAI[SYS,HE] blob sn#004215 filedate 1972-09-20 generic text, type T, neo UTF8
00100	COMMENT ⊗   VALID 00004 PAGES 
00200	RECORD PAGE   DESCRIPTION
00300	 00001 00001
00400	 00002 00002	⊃ Needs the following to be defined: ⊃, CRLF
00500	 00005 00003	SIMPLE PROCEDURE NORML(REAL XIB,YIB,XIC,YIC,XID,YID REAL ARRAY DIRE)
00600	 00008 00004	PROCEDURE POSE_HAND(REAL XH,YH,ZH,DIRX,DIRY REFERENCE INTEGER HNDFLG)
00700	 00010 ENDMK
00800	⊗;
     

00100	⊃ Needs the following to be defined: ⊃, CRLF;
00200	
00300	PROCEDURE IMAGECOR(REAL XTC,YTC,ZTC; REFERENCE REAL XIC,YIC);
00400	  ⊃ Transform table coords to image coords. use COL;
00500	  BEGIN "IMAGECOR"
00600		INTEGER I,J,K;
00700	        REAL AC;
00800	        REAL ARRAY V,U[1:3];
00900		AC←CAMERA_MODEL[4,3];
01000		V[1]←(CAMERA_MODEL[4,1]*ZTC-AC*XTC)/(ZTC-AC);
01100		V[2]←(CAMERA_MODEL[4,2]*ZTC-AC*YTC)/(ZTC-AC);
01200	        V[3]←1;
01300		FOR I←1 STEP 1 UNTIL 3 DO BEGIN 
01400		    AC←0;
01500	            FOR J←1 STEP 1 UNTIL 3 DO AC←AC+CAMERA_MODEL[I,J]*V[J];
01600		    U[I]←AC; END;
01700		XIC←U[1]/U[3]; YIC←U[2]/U[3];
01800	  END "IMAGECOR";
01900	
02000	PROCEDURE TABLECOR(REAL XIC,YIC; REFERENCE REAL XTC,YTC);
02100	  ⊃ Transform image coords to tabletop coords. use ICOL;
02200	  BEGIN "TABLECOR"
02300		INTEGER I,J,K;
02400	        REAL AC;
02500	        REAL ARRAY V,U[1:3];
02600		V[1]←XIC; V[2]←YIC; V[3]←1;
02700		FOR I←1 STEP 1 UNTIL 3 DO BEGIN 
02800		    AC←0;
02900	            FOR J←1 STEP 1 UNTIL 3 DO AC←AC+CAMERA_MODEL[5+I,J]*V[J];
03000		    U[I]←AC; END;
03100		XTC←U[1]/U[3]; YTC←U[2]/U[3];
03200	  END "TABLECOR";
03300	
03400	SIMPLE PROCEDURE HOME_ARM;
03500	  BEGIN "HOME_ARM"
03600	    MESS←ISSUE(5,"DRV","HAND",MESSAGE START_TRAJECTORY("MASLUL",FALSE));
03700	    MESS←ISSUE(5,"DRV","HAND",MESSAGE OPEN_HAND(3));
03800	    MESS←ISSUE(5,"DRV","HAND",MESSAGE PARK_ARM);
03900	    MESS←ISSUE(5,"DRV","HAND",MESSAGE CLOSE_TRAJECTORY);
04000	    MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_IT("MASLUL"));
04100	    IF   ARM_STATUS≠0
04200	    THEN OUTSTR("HOME_ARM: ARM FAILED (ARM_STATUS="&CVOS(ARM_STATUS)&")"CRLF);
04300	  END "HOME_ARM";
     

00100	SIMPLE PROCEDURE NORML(REAL XIB,YIB,XIC,YIC,XID,YID; REAL ARRAY DIRE);
00200	  BEGIN "NORML"
00300		REAL D;
00400		FORTRAN REAL PROCEDURE SQRT(REAL X);
00500	
00600	    D←SQRT((XIB-XIC)↑2+(YIB-YIC)↑2);
00700	    DIRE[1]←(XIB-XIC)/D; DIRE[2]←(YIB-YIC)/D;
00800	    D←SQRT((XID-XIC)↑2+(YID-YIC)↑2);
00900	    DIRE[3]←(XID-XIC)/D; DIRE[4]←(YID-YIC)/D;
01000	    IF (DIRE[1]*DIRE[4]-DIRE[2]*DIRE[3])<0
01100	    THEN BEGIN DIRE[1]↔DIRE[3]; DIRE[2]↔DIRE[4]; END;
01200	    DIRE[5]←XIC; DIRE[6]←YIC; DIRE[7]←0; DIRE[8]←0;
01300	  END "NORML";
01400	
01500	SIMPLE PROCEDURE ARMTRAN(REAL DX,DY,DZ; REFERENCE REAL XTC,YTC,ZTC);
01600	  BEGIN "ARMTRAN"
01700	  XTC←ARM_LINK[6,1,1]*DX+ARM_LINK[6,1,2]*DY+ARM_LINK[6,1,3]*DZ+ARM_LINK[6,1,4];
01800	  YTC←ARM_LINK[6,2,1]*DX+ARM_LINK[6,2,2]*DY+ARM_LINK[6,2,3]*DZ+ARM_LINK[6,2,4];
01900	  ZTC←ARM_LINK[6,3,1]*DX+ARM_LINK[6,3,2]*DY+ARM_LINK[6,3,3]*DZ+ARM_LINK[6,3,4];
02000	  END "ARMTRAN";
02100	
02200	SIMPLE PROCEDURE CORN_HAND(REAL DX,DY,DZ; REAL ARRAY DIRD);
02300	  BEGIN "CORN_HAND"
02400		REAL XTC,YTC,ZTC,XIB,YIB,XIC,YIC,XID,YID;
02500	    ARMTRAN(DX,DY,DZ,XTC,YTC,ZTC);
02600	    IMAGECOR(XTC,YTC,ZTC,XIC,YIC);
02700	    ARMTRAN(-DX,DY,DZ,XTC,YTC,ZTC);
02800	    IMAGECOR(XTC,YTC,ZTC,XIB,YIB);
02900	    ARMTRAN(DX,DY,DZ-.25,XTC,YTC,ZTC);
03000	    IMAGECOR(XTC,YTC,ZTC,XID,YID);
03100	    NORML(XIB,YIB,XIC,YIC,XID,YID,DIRD);
03200	  END "CORN_HAND";
03300	
03400	SIMPLE REAL PROCEDURE GRASP_CAL;
03500	  BEGIN REAL GCOR;
03600	    ARM_EXECUTE←S_O_T←TRUE;
03700	    MESS←ISSUE(7,"DRV","HAND",MESSAGE OPEN_HAND(-1));
03800	    IF   (ARM_STATUS=6)∨(ARM_STATUS='16)
03900	    THEN BEGIN GCOR←GRASP;
04000		   OUTSTR("GRASP_CAL: GCOR="&CVG(GCOR)CRLF);
04100	           MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_PROCEED); END
04200	    ELSE OUTSTR("GRASP_CAL-FAILED (ARM_STATUS="&CVOS(ARM_STATUS)&")"CRLF);
04300	    S_O_T←FALSE;
04350	    ARM_EXECUTE←TRUE;
04400	    MESS←ISSUE(7,"DRV","HAND",MESSAGE OPEN_HAND(3));
04500	    RETURN(GCOR);
04600	  END "GRASP_CAL";
     

00100	PROCEDURE POSE_HAND(REAL XH,YH,ZH,DIRX,DIRY; REFERENCE INTEGER HNDFLG);
00200	  BEGIN "POSE_HAND"
00300		REAL ARRAY TRANS[1:4,1:4];
00400	
00500	    HNDFLG←0;
00600	    TRANS[4,1]←TRANS[4,2]←TRANS[4,3]←0; TRANS[4,4]←1;
00700	    TRANS[1,3]←TRANS[2,3]←0; TRANS[3,3]←-1;
00800	    TRANS[3,1]←TRANS[3,2]←0;
00900	    TRANS[1,4]←XH; TRANS[2,4]←YH; TRANS[3,4]←ZH;
01000	    TRANS[2,1]←TRANS[1,2]←DIRX;
01100	    TRANS[1,1]←-(TRANS[2,2]←DIRY);
01200	    MESS←ISSUE(5,"DRV","HAND",MESSAGE START_TRAJECTORY("MASLUL",FALSE));
01300	    MESS←ISSUE(7,"DRV","HAND",MESSAGE MOVE_ARM(TRANS,ARM_PLAN));
01400	    IF   ARM_PLAN
01500	    THEN MESS←ISSUE(5,"DRV","HAND",MESSAGE CLOSE_TRAJECTORY)
01600	    ELSE BEGIN OUTSTR("POSE_HAND: THEN ARM CAN'T REACH THIS LOCATION"CRLF);
01700		       HNDFLG←1; RETURN; END;
01800	    MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_IT("MASLUL"));
01900	    IF ARM_STATUS≠0 THEN BEGIN
02000	       OUTSTR("POSE_HAND: ARM FAILED (ARM_STATUS"&CVOS(ARM_STATUS)&")"CRLF);
02100	       HNDFLG←2; RETURN; END;
02200	  END "POSE_HAND";